#include "angle_compare.hpp"
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <pcl/filters/voxel_grid.h>


double get_angle(int direct,double seat_start,double seat_end,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>); 
  pcl::PointXYZ p_start,p_end;
  double angle=100.0;
  
  if(direct==0) //x<0 ,y>seat_start y<seat_end
    {
        std::size_t size=cloud->points.size();
        for(std::size_t i=0;i<size;++i)
          if((cloud->points[i].x<0)&&(cloud->points[i].y>seat_start)&&(cloud->points[i].y<seat_end))
            cloud_temp->points.push_back(cloud->points[i]);
        if(cloud_temp->points.size()>3)
        {
            p_start=cloud_temp->points[0];
            p_end = cloud_temp->points[0];
            for(std::size_t i=0;i<cloud_temp->points.size();++i)
            {
                if(cloud_temp->points[i].y>p_end.y)p_end=cloud_temp->points[i];
                if(cloud_temp->points[i].y<p_start.y)p_start=cloud_temp->points[i];
            }
            
            angle=atan2(p_end.y-p_start.y,p_end.x-p_start.x);
         
        }    
    }
     
}
void global_json(std::string name)
{
     std::ifstream file(name); 
    if (!file.is_open())  {
        std::cerr << "Error: Failed to open JSON file!" << std::endl;
        return ;
    }
         // 步骤2：解析JSON内容 
    Json::Value root;
    Json::CharReaderBuilder readerBuilder;
    std::string parseErrors;
       // 新版JsonCpp解析方法（推荐）
    bool parsingSuccessful = Json::parseFromStream(
        readerBuilder, file, &root, &parseErrors
    );
    file.close();
    std::vector<std::string> result;
    for (const auto& item : root["map_name"]) {
         if (item.isString())  {
            result.push_back(item.asString());
         }
        }
   // for(int i=0;i<result.size();++i) std::cout<<result[i]<<std::endl;    
}
Compare_to_shift::Compare_to_shift()
{
  source_cloud.reset(new  pcl::PointCloud<pcl::PointXYZ>);
  target_cloud.reset(new  pcl::PointCloud<pcl::PointXYZ>);
  transformed_cloud0.reset(new  pcl::PointCloud<pcl::PointXYZ>);
  transformed_cloud1.reset(new  pcl::PointCloud<pcl::PointXYZ>);
  shift_data.angle=0;
  shift_data.x=0;
  shift_data.y=0;
  global_data.angle=0;  global_data.x=0;global_data.y=0;
}
void Compare_to_shift::set_source_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
      // 创建体素栅格滤波器对象
      pcl::VoxelGrid<pcl::PointXYZ> sor;
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
      std::size_t size;
      size=cloud->points.size();
      for(std::size_t i=0;i<size;++i)
        if((cloud->points[i].x<100)&&(cloud->points[i].x>-100))
          cloud_f->points.push_back(cloud->points[i]);
      sor.setInputCloud(cloud_f); 
      sor.setLeafSize(0.02f,  0.02f, 0.02f);  // 关键参数：体素尺寸（单位：米）
      source_cloud->clear();
      sor.filter(*source_cloud); 
  //pcl::copyPointCloud(*cloud,*source_cloud);
}
void Compare_to_shift::set_target_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  // 创建体素栅格滤波器对象
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
  std::size_t size;
  size=cloud->points.size();
  for(std::size_t i=0;i<size;++i)
    if((cloud->points[i].x<100)&&(cloud->points[i].x>-100))
      cloud_f->points.push_back(cloud->points[i]);
  sor.setInputCloud(cloud_f); 
  sor.setLeafSize(0.02f,  0.02f, 0.02f);  // 关键参数：体素尺寸（单位：米）
   target_cloud->clear();
   sor.filter(*target_cloud); 
 // pcl::copyPointCloud(*cloud,*target_cloud);
}
void Compare_to_shift::transform_cloud(int index,Shift_data &sf_d)
{
    // 创建变换矩阵 
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    // 添加平移分量 x,y,z
    transform.translation()  << sf_d.x, sf_d.y, 0.0;
    double angle_rad = sf_d.angle;      // * M_PI / 180.0;  // 转为弧度 
    // 绕Z轴旋转（适用于2D平面变换）
    transform.rotate(Eigen::AngleAxisf(angle_rad,  Eigen::Vector3f::UnitZ()));
    //pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>); 
    if(index==0)
    {
      transformed_cloud0->clear();
      pcl::transformPointCloud(*source_cloud, *transformed_cloud0, transform);
    }
    if(index==1)
    {
      transformed_cloud1->clear();
      pcl::transformPointCloud(*target_cloud, *transformed_cloud1, transform);
    }
}
//
void Compare_to_shift::filter_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  std::size_t size;
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    size=cloud->points.size();
    for(std::size_t i=0;i<size;++i)
      if((cloud->points[i].x<100)&&(cloud->points[i].x>-100))
        temp_cloud->points.push_back(cloud->points[i]);
    cloud->clear();    
    pcl::copyPointCloud(*temp_cloud,*cloud);
}
void Compare_to_shift::cal_angle(int direct,double seat_start,double seat_end,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>); 
  pcl::PointXYZ p_start,p_end;
  double angle=100.0;
  tem_shift_data.angle=0; tem_shift_data.x=0;tem_shift_data.y=0;
  if(direct==0) //后方
  {
    std::size_t size=cloud->points.size();
    for(std::size_t i=0;i<size;++i)
      if((cloud->points[i].x<0)&&(cloud->points[i].y>seat_start)&&(cloud->points[i].y<seat_end))
        cloud_temp->points.push_back(cloud->points[i]);
      if(cloud_temp->points.size()>3)
      {
          p_start=cloud_temp->points[0];
          p_end = cloud_temp->points[0];
          for(std::size_t i=0;i<cloud_temp->points.size();++i)
          {
              if(cloud_temp->points[i].y>p_end.y)p_end=cloud_temp->points[i];
              if(cloud_temp->points[i].y<p_start.y)p_start=cloud_temp->points[i];
          }
          
          angle=atan2(p_end.y-p_start.y,p_end.x-p_start.x);
       
      }    
 
  }    
  if(direct==2) //前方
  {
    std::size_t size=cloud->points.size();
    for(std::size_t i=0;i<size;++i)
      if((cloud->points[i].x>0)&&(cloud->points[i].y>seat_start)&&(cloud->points[i].y<seat_end))
        cloud_temp->points.push_back(cloud->points[i]);
      if(cloud_temp->points.size()>3)
      {
          p_start=cloud_temp->points[0];
          p_end = cloud_temp->points[0];
          for(std::size_t i=0;i<cloud_temp->points.size();++i)
          {
              if(cloud_temp->points[i].y>p_end.y)p_end=cloud_temp->points[i];
              if(cloud_temp->points[i].y<p_start.y)p_start=cloud_temp->points[i];
          }
          
          angle=atan2(p_end.y-p_start.y,p_end.x-p_start.x);
       
      }    
 
  }    
  if(direct==1)//右边
  {
    std::size_t size=cloud->points.size();
    for(std::size_t i=0;i<size;++i)
      if((cloud->points[i].y<0)&&(cloud->points[i].x>seat_start)&&(cloud->points[i].x<seat_end))
        cloud_temp->points.push_back(cloud->points[i]);
      std::cout<<"pointsize="<<cloud_temp->points.size()<<std::endl;
    if(cloud_temp->points.size()>3)
      {
          p_start=cloud_temp->points[0];
          p_end = cloud_temp->points[0];
      }    
    for(std::size_t i=0;i<cloud_temp->points.size();++i)
          {
              if(cloud_temp->points[i].x>p_end.x)p_end=cloud_temp->points[i];
              if(cloud_temp->points[i].x<p_start.x)p_start=cloud_temp->points[i];
          }
  }
  if(direct==3)//左边
  {
    std::size_t size=cloud->points.size();
    for(std::size_t i=0;i<size;++i)
      if((cloud->points[i].y>0)&&(cloud->points[i].x>seat_start)&&(cloud->points[i].x<seat_end))
        cloud_temp->points.push_back(cloud->points[i]);
      std::cout<<"pointsize="<<cloud_temp->points.size()<<std::endl;
    if(cloud_temp->points.size()>3)
      {
          p_start=cloud_temp->points[0];
          p_end = cloud_temp->points[0];
      }    
    for(std::size_t i=0;i<cloud_temp->points.size();++i)
          {
              if(cloud_temp->points[i].x>p_end.x)p_end=cloud_temp->points[i];
              if(cloud_temp->points[i].x<p_start.x)p_start=cloud_temp->points[i];
          }
  }
  tem_shift_data.angle=angle;
  tem_shift_data.y=(p_end.y+p_start.y)/2.0;
  tem_shift_data.x=(p_end.x+p_start.x)/2.0;
}

void Compare_to_shift::get_shift(void)
{
  Shift_data sfp1,sfp0;
  cal_angle(0,-2.0,0.5,source_cloud);
  sfp0=tem_shift_data;
  cal_angle(0,-2.0,0.5,target_cloud);
  sfp1=tem_shift_data;
//  std::cout<<"angle0= "<<sfp0.angle<<"  x0="<<sfp0.x<<" y0="<<sfp0.y<<std::endl;
//  std::cout<<"angle1= "<<sfp1.angle<<"  x1="<<sfp1.x<<" y1="<<sfp1.y<<std::endl;
  shift_data.angle+=sfp0.angle-sfp1.angle;
  shift_data.x += sfp0.x-sfp1.x;
  shift_data.y=0;
  transform_cloud(1,shift_data);  ///convert target_cloud

  cal_angle(0,-2.0,0.5,transformed_cloud1);
  sfp1=tem_shift_data;
//  std::cout<<"angle0= "<<sfp0.angle<<"  x0="<<sfp0.x<<" y0="<<sfp0.y<<std::endl;
//  std::cout<<"angle1= "<<sfp1.angle<<"  x1="<<sfp1.x<<" y1="<<sfp1.y<<std::endl;
  shift_data.angle+=sfp0.angle-sfp1.angle;
  shift_data.x+= sfp0.x-sfp1.x;
  shift_data.y=0;
   transform_cloud(1,shift_data);  ///convert target_cloud

   cal_angle(0,-2.0,0.5,transformed_cloud1);
   sfp1=tem_shift_data;
//  std::cout<<"angle0= "<<sfp0.angle<<"  x0="<<sfp0.x<<" y0="<<sfp0.y<<std::endl;
//  std::cout<<"angle1= "<<sfp1.angle<<"  x1="<<sfp1.x<<" y1="<<sfp1.y<<std::endl;
 shift_data.angle+=sfp0.angle-sfp1.angle;
  shift_data.x+= sfp0.x-sfp1.x;
  shift_data.y=0;
  // std::cout<<"angle2= "<<shift_data.angle<<"  x2="<<shift_data.x<<" y2="<<shift_data.y<<std::endl;
  //*target_cloud=*transformed_cloud1;
  //进行Y轴计算
  std::cout<<"start Y "<<std::endl;
  cal_angle(1,-1.2,0.2,source_cloud);
  sfp0=tem_shift_data;
  transform_cloud(1,shift_data);  ///convert target_cloud
  cal_angle(1,-1.2,0.2,transformed_cloud1);
  sfp1=tem_shift_data; 
  shift_data.y +=sfp0.y-sfp1.y;
  // std::cout<<"angle0= "<<sfp0.angle<<"  x0="<<sfp0.x<<" y0="<<sfp0.y<<std::endl;
  // std::cout<<"angle1= "<<sfp1.angle<<"  x1="<<sfp1.x<<" y1="<<sfp1.y<<std::endl;
   transform_cloud(1,shift_data);  ///convert target_cloud
  cal_angle(1,-1.2,0.2,transformed_cloud1);
   sfp1=tem_shift_data; 
  shift_data.y +=sfp0.y-sfp1.y;

  //再进行x
  // Shift_data sfp1,sfp0;
  cal_angle(0,-2.0,0.5,source_cloud);
  sfp0=tem_shift_data;
  transform_cloud(1,shift_data);  ///convert target_cloud
  cal_angle(0,-2.0,0.5,transformed_cloud1);
  sfp1=tem_shift_data;
//  std::cout<<"angle0= "<<sfp0.angle<<"  x0="<<sfp0.x<<" y0="<<sfp0.y<<std::endl;
//  std::cout<<"angle1= "<<sfp1.angle<<"  x1="<<sfp1.x<<" y1="<<sfp1.y<<std::endl;
  shift_data.angle+=sfp0.angle-sfp1.angle;
  shift_data.x += sfp0.x-sfp1.x;
  std::cout<<"angle0= "<<sfp0.angle<<"  x0="<<sfp0.x<<" y0="<<sfp0.y<<std::endl;
  std::cout<<"angle1= "<<sfp1.angle<<"  x1="<<sfp1.x<<" y1="<<sfp1.y<<std::endl;
   std::cout<<"angle22= "<<shift_data.angle<<"  x22="<<shift_data.x<<" y22="<<shift_data.y<<std::endl;
}
void Compare_to_shift::read_json_seat(void)
{
  double temp_double;
  std::ifstream file(json_name); 
    if (!file.is_open())  {
        std::cerr << "Error: Failed to open JSON file!" << std::endl;
        return ;
    }
      // 步骤2：解析JSON内容 
    Json::Value root;
    Json::CharReaderBuilder readerBuilder;
    std::string parseErrors;
       // 新版JsonCpp解析方法（推荐）
    bool parsingSuccessful = Json::parseFromStream(
        readerBuilder, file, &root, &parseErrors
    );
    for(int i=0;i<4;++i)
      {
        ss_seat[i].direct=100; ss_seat[i].seat_end = 0.0; ss_seat[i].seat_start = 0.0;
      }
      if (!parsingSuccessful) {
        std::cerr << "Parse error: " << parseErrors << std::endl;
        return ;
    }
    global_data.angle = root["seat0_angle"].asDouble();
    global_data.x = root["seat0_x"].asDouble();
    global_data.y = root["seat0_y"].asDouble();
    if(root.isMember("direct0"))
    {
      ss_seat[0].direct= root["direct0"].asInt();
      ss_seat[0].times = root["times0"].asInt();
      ss_seat[0].seat_start = root["seat_start0"].asDouble();
      ss_seat[0].seat_end = root["seat_end0"].asDouble();
     
    }
    if(root.isMember("direct1"))
    {
      ss_seat[1].direct= root["direct1"].asInt();
      ss_seat[1].times = root["times1"].asInt();
      ss_seat[1].seat_start = root["seat_start1"].asDouble();
      ss_seat[1].seat_end = root["seat_end1"].asDouble();
    }
    if(root.isMember("direct2"))
    {
      ss_seat[2].direct= root["direct2"].asInt();
      ss_seat[2].times = root["times2"].asInt();
      ss_seat[2].seat_start = root["seat_start2"].asDouble();
      ss_seat[2].seat_end = root["seat_end2"].asDouble();
    }
    if(root.isMember("direct3"))
    {
      ss_seat[3].direct= root["direct3"].asInt();
      ss_seat[3].times = root["times3"].asInt();
      ss_seat[3].seat_start = root["seat_start3"].asDouble();
      ss_seat[3].seat_end = root["seat_end3"].asDouble();
    }
    file.close();
    for(int i=0;i<4;++i)  ///纠正大小数
    {
       if(ss_seat[i].seat_start>ss_seat[i].seat_end)
        {temp_double=ss_seat[i].seat_start;
          ss_seat[i].seat_start=ss_seat[i].seat_end;
          ss_seat[i].seat_end=temp_double;
        }
    }
}
int Compare_to_shift::get_comp_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  int cloud_size=0;
  for(int i=0;i<4;++i)
  {
    std::size_t size=cloud->points.size();
    if(ss_seat[i].direct==0)//后方
      {
        for(std::size_t j=0;j<size;++j)
          if((cloud->points[j].x<0)&&(cloud->points[j].y>ss_seat[i].seat_start)&&(cloud->points[j].y<ss_seat[i].seat_end))
            {cloud->points[j].z = 0.30;cloud_size++;}
            
      }
     if(ss_seat[i].direct==1)//右方
      {
        for(std::size_t j=0;j<size;++j)
          if((cloud->points[j].y<0)&&(cloud->points[j].x>ss_seat[i].seat_start)&&(cloud->points[j].x<ss_seat[i].seat_end))
               {cloud->points[j].z = 0.30;cloud_size++;}
      }  
     if(ss_seat[i].direct==2)//前方
      {
        for(std::size_t j=0;j<size;++j)
           if((cloud->points[j].x>0)&&(cloud->points[j].y>ss_seat[i].seat_start)&&(cloud->points[j].y<ss_seat[i].seat_end))
               {cloud->points[j].z = 0.30;cloud_size++;}
      } 
     if(ss_seat[i].direct==3)//左方
      {
        for(std::size_t j=0;j<size;++j)
            if((cloud->points[j].y>0)&&(cloud->points[j].x>ss_seat[i].seat_start)&&(cloud->points[j].x<ss_seat[i].seat_end))
                {cloud->points[j].z = 0.30;cloud_size++;}
      }   
  }
  return cloud_size;
}   

void Compare_to_shift::convert_json_name(std::string name)
{
   // 查找最后一个'.'的位置
    std::size_t dot_pos = name.find_last_of('.');
    json_name = name.substr(0,  dot_pos) + ".json";
}
void Compare_to_shift::get_convert_json(void)
{
  Shift_data sfp1,sfp0;
  for(int i=0;i<4;++i)
  {
    if((ss_seat[i].direct!=100)&&(ss_seat[i].times>0))
    {
      cal_angle(ss_seat[i].direct,ss_seat[i].seat_start,ss_seat[i].seat_end,source_cloud);
      sfp0=tem_shift_data;
      for(int j=0;j<ss_seat[i].times;++j)
      {
        transform_cloud(1,shift_data);  ///convert target_cloud
        cal_angle(ss_seat[i].direct,ss_seat[i].seat_start,ss_seat[i].seat_end,transformed_cloud1);
        sfp1=tem_shift_data;
        if((ss_seat[i].direct==0)||(ss_seat[i].direct==2))
        {
          shift_data.angle+=sfp0.angle-sfp1.angle;
          shift_data.x += sfp0.x-sfp1.x;
          //shift_data.y=0;
        }
        if((ss_seat[i].direct==1)||(ss_seat[i].direct==2))
        {
          sfp1=tem_shift_data; 
          shift_data.y +=sfp0.y-sfp1.y;
        }
      }

    }
  }
  
}